#ifndef CONTROL_HPP
#define CONTROL_HPP

#include "calibrate.hpp"
#include "tool.hpp"
#include <vector>
#include <iostream>
#include <yaml-cpp/yaml.h>
#include <fstream>


using std::cout;
using std::endl;

struct ControlInput{
    bool control_enable;
    bool stop_flag;
    float vehicle_velocity_kph;
    float pitch_angle;
    float roll_angle;
    float expect_distance;
    float expect_velocity_kph;
    float expect_accelaration;
    float c0;
    float c1;
    float c2;
};

struct ControlOutput{
    float steer_angle;
    float throttle;
    float brake_value;
    float brake_enable_flag;
};

class Control{
public: 
    // ControlOutput CalculateControl(const ControlInput& control_input);
    ControlOutput CalculateControl();
private:
    void SetInput();
    void TrajectoryToAccelaration();
    void VelocitySolveAccelaration();
    void ThrottleFeedForward();
    void ThrottleFeedBack();
    void AngleFeedback();
    void AngleFeedforward();
    void AngleOutDeal();
    void BrakeControl();
    Tool tool;
    ControlInput input;
    YAML::Node read_input;
    struct InternalState{
        float expect_accelaration;
        float ego_accelaration;
        float throttle_feedforward;
        float throttle_final;
        float brake_value;
        float brake_enable_flag;
        float angle_feedback;
        float angle_feedforward;
        float angle_out;
    }internal_state;
};

#endif